#include "WPILib.h"
#include "2489Robot.h"

/**
 * Assumes: RobotDrive class myRobot
 * -or-
 * four jaguar classes: fleftjaguar, bleftjaguar, frightjaguar, brightjaguar
 * 
 * set tank style drive with #define TANK_DRIVE
 */

/**
 * Runs the motors with arcade steering. 
 *
void Robot2489::OperatorControl(void)
{
	GetWatchdog().SetEnabled(true);
	while (IsOperatorControl())
	{
		GetWatchdog().Feed();
		myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
		Wait(0.005);				// wait for a motor update time
	}
}
*/

void Robot2489::OperatorControl(void)
{
	double motorsetl, motorsetr;
	GetWatchdog().SetEnabled(true);
	while(IsOperatorControl())
	{
		GetWatchdog().Feed();
		#ifdef TANK_DRIVE
		motorsetl = MotorControl(leftstick.GetY());
		motorsetr = MotorControl(rightstick.GetY());
		fleftjaguar.Set(motorsetl);
		bleftjaguar.Set(motorsetl);
		frightjaguar.Set(motorsetr);
		brightjaguar.Set(motorsetr);
		Wait(0.005);
		#else
		myRobot.Drive(MotorControl(rightstick.GetY()),rightstick.GetX());
		Wait(0.005);
		#endif
	}
}
